`timescale 1ns / 1ns

module my_atan
(
    input  wire                  CLK_50M,
    input  wire                  RST_N,
    input  wire                  be_flag,
    input  wire  [23:0]          x_data,
    input  wire  [23:0]          y_data,
    input  wire  [23:0]          t_data,
    input  wire                  mode, // 0:向量 1:旋转
    output reg   [23:0]          x_out,
    output reg   [23:0]          y_out,
    output reg   [23:0]          z_out,
    output reg                   flag
);

parameter   S1         =  4'b0001;
parameter   S2         =  4'b0010;
parameter   S3         =  4'b0100;
parameter   S4         =  4'b1000;
parameter   jiaozheng  =  24'd2547003; // 校准值 0.607


// 主控
reg         [3:0]       state       ;

reg         [4:0]       times       ; // 迭代次数
reg         [4:0]       addr_r      ; // 地址
reg	signed  [24:0]      xn_r		; // 1:1:22
reg	signed  [24:0]      yn_r		; // 1:1:22
reg	signed  [23:0]      zn_r        ; // 1:1:22
wire  		[23:0]      angle       ; // 每一次对应的角度 1:7:16
wire                    sign_data   ;

assign sign_data = mode ? !zn_r[23] : yn_r[23];
rom_atan	rom_atan_inst
(
	.address    (addr_r),
	.clock      (CLK_50M),
	.q          (angle)
);

/* 主控状态机 */
always @ (posedge CLK_50M or negedge RST_N) begin
    if(!RST_N) begin
        state   <= S1   ;
        xn_r    <= 0    ;
        yn_r    <= 0    ;
        zn_r    <= 0    ;
        times   <= 0    ;
        addr_r  <= 1    ; // ROM地址
        flag    <= 0    ;
        x_out   <= 0    ;
        y_out   <= 0    ;
        z_out   <= 0    ;
    end else begin
        casex(state)
        S1: begin // 默认状态
            if(be_flag) begin
                if(mode) begin
                    xn_r    <= jiaozheng;
                    yn_r    <= 0        ;
                    zn_r    <= t_data   ;
                end else begin
                    xn_r    <= x_data   ;
                    yn_r    <= y_data   ;
                    zn_r    <= 0        ;
                end
                state   <= S2       ;
            end else begin
                flag    <= 0        ;
                times   <= 0        ;
                addr_r  <= 0        ; // ROM地址
            end
        end
        S2: begin // 等待
            addr_r  <= addr_r + 1'd1;
            state   <= S3        ;
        end
        S3: begin
            if(times < 5'd23) begin
                // 如果这个改了，迭代值就要改
				// if(mode || (yn_r != 0)) begin //当旋转到y=0时，提前结束，否则继续旋转反而影响精度
					if(sign_data) begin//yn最高位为1时，即坐标在第四象限，则逆时针旋转
						xn_r <= xn_r - (yn_r >>> times);
						yn_r <= yn_r + (xn_r >>> times);
						addr_r <= addr_r + 1'd1;
						times <= addr_r;
						zn_r <= zn_r - angle;
					end else begin									//反之，坐标在第一象限，则顺时针旋转
						xn_r <= xn_r + (yn_r >>> times);
						yn_r <= yn_r - (xn_r >>> times);
						addr_r <= addr_r + 1'd1;
						times <= addr_r;
						zn_r <= zn_r + angle;
					end
				// end else begin // 提前结束
				// 	state <= S4;
				// end
            end else begin // 迭代完成，进入下一状态
				state <= S4;
			end
        end
        S4: begin
            x_out <= xn_r[23:0];
            y_out <= yn_r[23:0];
            z_out <= zn_r;
            flag  <= 1;
            state <= S1;
        end
        endcase
    end
end

endmodule
